#include "bsp_servo.h"

Adafruit_PWMServoDriver servo = Adafruit_PWMServoDriver();

void Servo_Init()
{
  servo.begin();
  servo.setPWMFreq(50);
  ServoRun(LIFT);      
}

void ServoRun(uint8_t LiftOrDown)
{
    if(LiftOrDown == LIFT)
    {
        servo.setPWM(SERVO_L,0,SERVO_L_LIFT);
        servo.setPWM(SERVO_R,0,SERVO_R_LIFT);
    }
    else if(LiftOrDown == DOWN)
    {
        servo.setPWM(SERVO_L,0,SERVO_L_DOWN);
        servo.setPWM(SERVO_R,0,SERVO_R_DOWN);
    }
    else if(LiftOrDown == CLIMB)
    {
        servo.setPWM(SERVO_L,0,SERVO_L_CLIMB);
        servo.setPWM(SERVO_R,0,SERVO_R_CLIMB);
    }
}
